/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef ROSGUIAPI_H
#define ROSGUIAPI_H
#include <string>
#include <vector>
#include <ros/ros.h>
#include <math.h>
#include <vector>
#include <sstream>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <actionlib_msgs/GoalID.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdlib.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/UInt32MultiArray.h>
#include <tcr100/VehicleEncoder.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tcr100/VehicleGoal.h>
#include <amcl/AmclExtension.h>
#include "ros/master.h"
#include "ros/this_node.h"
//#include <tf/transform_broadcaster.h>
#include <QThread>
#include <QObject>
#include <QTimer>
#include "GeneralDefine.h"
#include "gSoapFoundation/soapStub.h"

#define D_SET_ODOMETER_COVE_RATIO 0.1

using namespace std;
//#include "gSoapFoundation/soapStub.h"
//struct VehicleVelocity
//{
//    double x_move;//according to vehicle unit in m/s
//    double y_move;
//    double z_rotate;//unit is rad/s
//};

//struct VehiclePosition
//{
//    double x;//m
//    double y;//m
//    double rotate;//unit is rad
//};

//namespace sick_tim {
//class SickTimCommon;
//class SickTim5512050001Parser;
//}
class AmclNode;
class AmclConfig;
class LaserCalculate;
class PclIcp;
class CLaserOdometry2D;
class VisionMatch;
class ReflectorMatch;

namespace KDL{
class Frame;
}

class LaserDriverAbstract;


/**
 * @brief The RosApi class agv通用服务接口类
 */
class RosApi: public QObject
{
    Q_OBJECT
public:
    RosApi(QString defaultConfigFilePathIn);
    ~RosApi();
public:
    int getReflectLaserFilterParameter(int index,struct LaserFilterParameter &paraOut);
    int setReflectLaserFilterParameter(int index,struct LaserFilterParameter  paraIn);
    int getReflectCalculateCommonParameter(ReflectCommonDetectPara &paraOut);
    int setReflectCalculateCommonParameter( ReflectCommonDetectPara paraIn);
    int getAmclDebugInfo(AmclDebugInfo &debugInfoOut);
    int setAmclPara(struct AmclDebugPara paraIn);
    int getAmclPara(struct AmclDebugPara &paraOut);
    int getLaserScanParameter(int laserId,struct LaserScanParameterHmi &infoOut);
    int setLaserScanParameter(int laserId,struct LaserScanParameterHmi infoIn,int &returnFlag);

    int getMapPropertyInfo(std::string mapName,struct MapPropertyInfo &infoOut);
    int setVisionMatchKalmanFilterPara(int filterId, double followQ, double observeR);
    int setReflectInitialVehiclePosition(VehiclePosition agvPoseIn);
    int setReflectMatchDebugFlag(int flagIn);
    int getReflectPositionCertainity(VehiclePosition& uncertainPoseOut);
    int getVmarkPositionCertainity(VehiclePosition &uncertainPoseOut);
    int getAllReferenceMarks(std::vector<ReflectMarkInfo> &reflectMarksOut);
    int addReferenceMarkToMap(ReflectMarkInfo markIn);
    int saveReferenceMarkToFile(string mapFileName_yaml);
    int deleteReferenceMarkInMap(int uniqId);
    int setDebugFlag(int flagIn);
    int setAmclIsCalculateLaser(bool isCalculateLaserIn);//设置是否计算激光数据
    bool isTfOkStatus();
    int getLaserLoseStatus(QVector<bool> &isLaseLoseListOut);
    int getInternalLaserDriverStatus(QVector<int> &statusOut);
    bool isInCreatingMapStatus();
    int setLineFittingMethod(int methodIn);
    int setIsPrintVmarkProcessData(bool isPrint);
    int setOriginalLaserFilterType(E_LASER_FILTER_TYPE typeIn);
    int getCurrentOriginalLaserFilterType(E_LASER_FILTER_TYPE &typeOut);
    int updateVehicleVelocity(VehicleVelocity velocityIn,struct timespec velocityUpdataStampIn);
    int getVMarkPositionInWorld(VehiclePosition &positionOut,bool &isSucess);
//    int setCheckVehiclePosition_vmark(bool isCheck);
    int setLaserIntensityFilterByType_visionMatch(int typeIn);
    int getMatchDebugInfo_vmark(VisionMatchDebugInfo &infoOut);
    int getMatchDebugInfo_reflect(ReflectDebugInfo &infoOut);
    int calculateVMarkPosition_byLaser(double baseLink_x,double baseLink_y,double baseLink_r,
                                       vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int updateVMarkFile(QString vmarkFileIn);
    int createVMark(VMarkInfo infoIn,QString vmarkFileIn);
    int getVmarkInfoList(QMap<int, VMarkInfo> &vmarkInfoOut);
    int setCurrentVMarkIndex(int indexIn);
    int setCurrentVMarkType(int currentVmarkTypeIn);
    int detectVMark_baseMap( vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int detectVMark_laser(int vmark_index, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int showDebugWindow_visionMatch(int id);
    int setVmarkDetectParameter(VMarkDetectParameter vmarkParameterIn);
    int getTimeConsumed(int componentId,double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int resetRosMeasureTime();
    int getAllPointCloud_translated(std::vector<PointCloudData_3D> &pointCloudOut);
    int getAllPointCloud_translated_qvector(QVector<PointCloudData_3D> &pointCloudOut);
    int getAllPointCloud_translated(PointCloudData_3D &pointCloudOut);
    int getPointCloud(std::string deviceName,PointCloudData_3D &pointCloudOut);
    int setVehiclePoseEstimation(double x,double y,double r);
    int forceAmclUpdatePartial();
    int setPclIcpWork(bool isWork);
    int setVmarkWork(bool isWork);
    int setReflectWork(bool isWork);
    int updatePclIcpDebugWindow();
    int setMatchCalculateParameter_pcl(PclCalculationParameter paraIn);
    int getMatchDebugInfo_pcl(PclMatchDebugInfo& infoOut);
    int showVisionMatchDebugWindow_pcl(bool isShowBaseMap, bool isShowBaseCompareMap, bool isShowLocalMap,
                                   bool isShowLocalCompareMap);
    int calculateVehiclePosition_pcl(MapRegion baseRegionIn,MapRegion localRegionIn);

    int setBaseMapRegion_pcl(MapRegion baseRegionIn);
    int setLocalMapRegion_pcl(MapRegion localRegionIn);
    int setBaseMapRegion_visionMatch(MapRegion baseRegionIn);
    int setLocalMapRegion_visionMatch(MapRegion localRegionIn);
    int setLocalMapLaserRegion_visionMatch(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn);
    int setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn);

    int shutDownRosModule();



    //***************接口函数部分********************************
    int getFrontLaserData(LaserDataInfo &frontLaserDataOut);
    int getAmclMatchScore(double &mapDistance_out, float &weight_out, std::vector<float> &pos_cov_out);
    int setDefaultMapName(std::string fileNameIn,int &returnFlag);

    //地图
    int getCurrentMapName(std::string &mapNameOut);
    int setMapResolution(double cmPerPixIn);//need todo hualei. 需要同步到ｇmapping配置文件，对建立地图有效
    int setMapSolver(struct UserInfo userInfo,enum E_MAP_SOLVER solverIn,int laserCount,int &returnFlag);//建图选择 enum 建图方法 int 雷达数量 int 返回标志
    int createMapStart(struct UserInfo userInfo, int &returnResult);//开始建图 string 地图名称 int 返回标志
    int createMapEnd(struct UserInfo userInfo,std::string mapName,int &returnResult);//结束建图 string 地图名称 int 返回标志
    int loadMap(struct UserInfo userInfo,std::string mapFileName,int &returnResult);//加载建图 string 地图名称 int 返回标志
    int loadReflectMap(std::string mapFileName_yaml);

//    //添加虚拟墙
    int addVitrualBlock(struct UserInfo userInfo,std::string configFilePath,int &returnFlag);
    int deleteVitrualBlock(struct UserInfo userInfo,int &returnFlag);
//    int addForbidenZone(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag);

//    //定位
    int setOdomSolver(struct UserInfo userInfo,enum E_ODOM_SOLVER solverIn,int &returnFlag);//选择odom enum 方法 int 返回标志
    int setLocationSolver(struct UserInfo userInfo,enum E_LOCATION_SOLVER solverIn,int &returnFlag);//选择定位算法 enum 定位方法 int 返回标志
    /**
     * @brief getVehiclePosition
     * @param userInfo
     * @param positionReturn ,unit is m   and  rad
     * @return
     */
    int getVehiclePosition_ros(struct UserInfo userInfo, PositionInfo &positionInfoOut);//获取agv位置 struct 位位置
    int getVehiclePosition_pclicp(struct UserInfo userInfo, PositionInfo &positionInfoOut);//获取agv位置 struct 位位置
    int getVehiclePositionInfo_vmark(struct UserInfo userInfo, PositionInfo &positionInfoOut);//获取agv位置 struct 位位置
    int getVehiclePositionInfo_reflect(struct UserInfo userInfo, PositionInfo &positionInfoOut);

    int updateVehiclePositionOnce();

    int setVehiclePosition(struct UserInfo userInfo, struct VehiclePosition positionIn,
                           double coveRatioIn, int &returnResult);//设置agv初始位置 struct 位位置 int 返回标志

//    //轨迹规划和行驶
    int setTrajectoryGlobalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_GLOBAL_SOLVER solverIn,int &returnFlag);//选择全局规划器 enum 全局规划器 int 返回标志
    int setTrajectoryLocalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_LOCAL_SOLVER solverIn,int &returnFlag);//选择局部规划器 enum 局部规划器 int 返回标志
    int setReplanCoutLimitWhenFailure(struct UserInfo userInfo,int countLimit,int &returnFlag);//设置失败重试次数 int 次数

    int startNavigation(struct UserInfo userInfo,int &returnFlag);//start navigation
    void startNavigation_bySignal();
    int stopNavigation(struct UserInfo userInfo,int &returnFlag);//stop navigation

    void moveToPosition_bySignal(VehiclePosition positionIn,double velocity);
    int moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag);//M/S  导航到指定点 struct 目标位置 double 车辆速度 int 返回标志
    int moveFollowPath(struct UserInfo userInfo,std::string filePath,std::string trajectoryName,double nearbyDistance,double velocity,int &returnFlag);//多点巡航  std::vector<struct VehiclePosition> 路径 double 速度
    int changeVelocity(struct UserInfo userInfo,double velocity,int &returnFlag);//设置速度 double 速度 int 返回标志
    int getMovingStatus(struct UserInfo userInfo,enum E_GOAL_STATUS& status);//获取导航状态 enum 状态
    int getRemainningTime(struct UserInfo userInfo,double &timeOut);//获取剩余时间  double 剩余时间
    int getRemainningDistance(struct UserInfo userInfo,double &distanceOut);//获取剩余距离  double 剩余距离
    int cancelMotion(struct UserInfo userInfo,int &returnFlag);//取消当前导航目标 int 返回标志

//    //设置
    int setPositionArrivedAccuracy(struct UserInfo userInfo,double distanceErrorIn,double angularErrorIn,int &returnFlag);//设置导航允许误差 double xy距离误差 double 航向误差 int 返回标志


    int setLocalPlanRange(struct UserInfo userInfo,double x_rangeIn,double y_rangeIn,int &returnFlag);//设置局部地图大小 double 长 double 宽
    int setVehicleBorder(QString configFileName);
    int setVehicleBorder(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag);//设置车体的footprint std::vector<double>x坐标 std::vector<double>y坐标
    int setLocalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag); //设置膨胀区大小 double 膨胀距离 int 返回标志
    int setGlobalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag); //设置膨胀区大小 double 膨胀距离 int 返回标志
    int startPathFollowMotion(UserInfo userInfo,int &returnFlag);
    int ceaseMotion(UserInfo userInfo,int &returnFlag);
    int startMotion(UserInfo userInfo,int &returnFlag);

//    int resetAll(struct UserInfo userInfo,int &returnFlag);//重置
//    //todo 规划器的参数

//    //状态
//    int getRosPackageStatus(enum E_ROS_PACKAGE package,enum E_ROS_PACKAGE_STATUS &status);//获取ROS包的状态 enum 状态


    //发布编码器ｏｄｏｍ　ｒｏｓ消息
    int setWheelOdometerData(struct OdemeterData odemeterDataIn);
    int setWheelOdometerData_noRos(UserInfo userInfo, int robotIdIn, struct OdemeterData odemeterDataIn, int &returnFlag);

    int getVehicleVelocityComand(UserInfo userInfo,int robotIdIn,struct  VehicleVelocity &velocityComamnd);

    int updatePciIcpPosition();
    int updateVmarkPosition();
    int updateReflectPosition();
    int getVehiclePositionFrom_tf(PositionInfo &positionInfoOut);
    int getVehiclePositionFrom_amcl(PositionInfo &positionInfoOut);
    int getVehiclePositionFrom_ros(PositionInfo &positionInfoOut);


 signals:
    void startNavigation_signal();
    void moveToPosition_signal(VehiclePosition positionIn,double velocity);

private slots:
    void timeOut_slot();
    void startNavigation_slot();
    void moveToPosition_slot(VehiclePosition positionIn,double velocity);

private:
    int checkYamlMap(std::string mapFileName_yaml);
    int readYamlFile(QString yamlFilePath, double &mapResolutionOut, double &xOffsetOut,
                     double &yOffsetOut, double &rOffsetOut, QString &imageNameOut);
    int getMapFileSize(QString pgmFilePath, double &width, double &heighth);
    int checkLaserDriverLoseData();
    int broadcastStaticTransform();
    int syncEncoderOdom(VehiclePosition startOdome);
    int rosLaunchCommand(QString commandStr);
    int initalLaserDevice(std::vector<LaserScanDeviceInfo> &laserScanDevice_infoListOut);
    int inital3dCameraDevice(std::vector<LaserScanDeviceInfo> &threeDCameraDevice_infoListOut);
    int loadMapFromYaml(QString yamlFilePath);

    int getFrontFilterLaserData(LaserDataInfo & frontFilterLaserDataOut);
    int addVitrualBlockFromMapFile(QString mapFilePath);
    int loadDefaultNavagationParameter(QString configFilePath);
    int readConfigFile(QString configFilePathIn);
    void rosLoop();
    void killFunction();
    bool init();
    bool ros_init();
    bool initial_message();
    int sendPositionMessage(VehiclePosition positionIn);
    void moveBaseResultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr& msg);
    void amcl_match_scoreCallback(const amcl::AmclExtensionConstPtr& msg);
    void twistCmdVelCallback(const geometry_msgs::Twist &msg);
    void frontLaserScanDataCallback(const sensor_msgs::LaserScanConstPtr& laser_scan);
    void backLaserScanDataCallback(const sensor_msgs::LaserScanConstPtr& laser_scan);
    void front3dCameraDataCallback(const sensor_msgs::PointCloud2ConstPtr& data_pointCloudIn);//todo
    void back3dCameraDataCallback(const sensor_msgs::PointCloud2ConstPtr& data_pointCloudIn);//todo
    int setVelocity(double velocity);//set velocity
    int setNearbyDistance(double distance);//set nearby distance
    bool moveToGoal(struct VehiclePosition positionIn);
    void gloablPathLenghtCallback(const nav_msgs::Path& path);
    void movebaseGoalCallback(const move_base_msgs::MoveBaseActionGoal& goal);


    void robotPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& pose);

    void* threadRun(void *);//更新vmark位置
    static void* threadTask( void* classPtr );
    void createThread();

    void* threadRun2(void *);//更新amcl位置,获取tf.
    static void* threadTask2( void* classPtr );
    void createThread2();

    int saveCarBorder(std::vector<double>point_x,std::vector<double>point_y);

    int readLaserScanConfigFile(QString configFilePathIn ,std::vector<LaserScanDeviceInfo> &infoListOut);
    int read3dCameraConfigFile(QString configFilePathIn ,std::vector<LaserScanDeviceInfo> &infoListOut);
//    int initialSickTimDriver();
//    int shutdownSickTimDriver();
//    int sickTimDriverLoopOnce();
//    void update_laser_driver_config(LaserDriverConfig configIn);
    int initialAmclAdvance();
    int initialLaserOdometry();
    int amclAdvanceLoopOnce();

    int insertPointCloud(PointCloudData_3D &target, const PointCloudData_3D &source);

    /**
     * @brief addMsg  , 添加消息反馈
     * @param messageLevel
     * @param componentName
     * @param messageType
     * @param messageCode
     * @param robotId
     * @param parameter1
     * @param parameter2
     * @param parameter3
     * @param parameter4
     */
    void addMsg( int messageLevel,
                 std::string componentName,
                 std::string messageType,
                 int messageCode,
                 int robotId,
                 int parameter1 = 0,
                 int parameter2 = 0,
                 int parameter3 = 0,
                 int parameter4 = 0,
                 QString message= "");


private:
    int robotId;
    QTimer timer;
    pthread_t threadId;
    pthread_t threadId2;
    int map_selector;
    int localization_algorithm;
    int local_trajectory_selector;
    int global_trajectory_selector;
    int nav_status;
    const char* map_to_load;
    double path_lenght;

    ros::NodeHandle nh_;
    ros::Rate rosRate;
    move_base_msgs::MoveBaseGoal *current_goal;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client_;
    ros::Publisher *initial_position_pub_;
    ros::Publisher *goal_pub_;
    ros::Publisher *front_filter_laser_pub_;
    ros::Subscriber *amcl_match_score_sub_;
    ros::Subscriber *move_base_status_sub_;
    ros::Subscriber *move_base_path_sub_;
    ros::Subscriber *move_base_goal_sub_;
    ros::Subscriber *cmd_vel_;
    ros::Subscriber *pose_sub_;
    ros::Subscriber *front_laser_data_sub_;
    ros::Subscriber *back_laser_data_sub_;
    ros::Subscriber *front_3dCamera_data_sub_;//todo
    ros::Subscriber *back_3dCamera_data_sub_;//todo
    ros::Publisher *odom_pub_;
    ros::Publisher *creatMap_pub_;
    ros::ServiceClient *amcl_service;
    tf::TransformListener *listener;
    tf::TransformBroadcaster *tfBroadcaster;

    VehicleVelocity rosVehicleCmdVel;

    PositionInfo vehiclePositionInfo_ros;
    PositionInfo vehiclePositionInfo_pclicp;
    PositionInfo vehiclePositionInfo_vmark;
    PositionInfo vehiclePositionInfo_reflect;


    QThread *currentThread;
    int msqid;

    QString defaultConfigFilePath;
    QString currentMapFileName;
    QString carBorderFileName;
    E_MAP_SOLVER mapSolver;
    E_ODOM_SOLVER odomSolver;
    E_LOCATION_SOLVER locationSolver;
    E_TRAJECTORY_GLOBAL_SOLVER trajectoryGlobalSolver;
    E_TRAJECTORY_LOCAL_SOLVER trajectoryLocalSolver;
    double localPlanXRange;
    double localPlanYRange;
    double localBarrierOffset;
    double globalBarrierOffset;
    int replanCountLimit;
    double arrivedAccuracy_linear;
    double arrivedAccuracy_angular;
    double currentSettingVelocity;

    UserInfo myUserInfo;
    int myReturnKey;
    bool isShutDown;

    double amcl_match_score;
    std::vector<float> amcl_pos_cov;

    LaserDataInfo *frontOriginLaserData;
    LaserDataInfo * frontFilterLaserData;
    LaserDataInfo *backOriginLaserData;
    PointCloudData_3D *frontOriginLaserData_pointCloud;

    PointCloudData_3D *frontFilterLaserData_pointCloud;
    PointCloudData_3D *backOriginLaserData_pointCloud;

    PointCloudData_3D *front_3dCameraData;
    PointCloudData_3D *back_3dCameraData;
    PointCloudData_3D *frontOriginLaserData_pointCloud_translated;
    PointCloudData_3D *frontFilterLaserData_pointCloud_translated;
    PointCloudData_3D *backOriginLaserData_pointCloud_translated;
    PointCloudData_3D *front_3dCameraData_translated;
    PointCloudData_3D *back_3dCameraData_translated;
    pthread_mutex_t rosDataMutex;
    pthread_mutexattr_t rosDataAttr;

    LaserCalculate* laserCalculate;

    PclIcp* pclMatch;

    VisionMatch* visionPositionMatch;
    ReflectorMatch* reflectPositionMatch;
    E_LASER_FILTER_TYPE originalLaserFilterType;
    MapRegion baseMapRegion;
    MapRegion localMapRegion;

//    bool isCheckVehiclePosition_vmark;
//    sick_tim::SickTimCommon* sickTimDriver;
//    sick_tim::SickTim5512050001Parser* parser;
    bool isUseSickTimDriver;
    AmclConfig* amclConfig;
    AmclNode* amclAdvance;
    bool isUseAmclAdvance;
    bool isUseVmarkDetect;
    bool isUseReflectDetect;
    bool isUsePclIcp;
    bool isPclIcpWork;
    bool isVmarkWork;
    bool isReflectWork;

    CLaserOdometry2D* laserOdometry;
    int skipFrameCount;
    int skipLaserPointCount;
    bool isUseLaserOdometry;
    PclCalculationParameter pclCalculationPara;
    bool isRosLoopFinished;


    std::vector<LaserScanDeviceInfo> *laserScanDevice_infoList;//注意多线程安全风险，没有加锁
    std::vector<LaserDriverAbstract*> internalLaserScanDriver;//驱动指针
    std::vector<LaserScanDeviceInfo> *threeDCameraDevice_infoList;
    QVector<QString> ros_launch_command;

    int visionPositionUpdateFailedCount;
    double pointCloudTrans_minConsumed;
    double pointCloudTrans_currentConsumed;
    double pointCloudTrans_maxConsumed;

    double rosThread_minConsumed;
    double rosThread_currentConsumed;
    double rosThread_maxConsumed;

    KDL::Frame *frame_origin_encoderOdom;
    KDL::Frame *frame_compensation_encoderOdom;
    VehiclePosition odemeterFinalData_encoder;
    bool isEncoderOdomPublish;
    bool isMainProgramInitialOk;

    VehicleVelocity currentVehicleVelocity;
    struct timespec velocityUpdataStamp;
    int amclMaxDogCount;
    int amclInitialPoseCalculateCount;
    bool isInCreatingMap;//是否正在创建地图。
    int rosTimer;//ros::spin定时器时间长度
    OdemeterData encoder_odemeterData;//编码器里程计
    bool isUseRosLaserMsg;
    int internalLaserDriverStatus[10];//
    int laserAllowTimeOutCount;
    int laserErrorTimeOutCount;
    int laserDriverLoseDataCount[10];
    bool isTfOk;
    bool isLaserLose;
    int vmarkCalculateSkip;
    int reflectCalculateSkip;
    int debugFlag;
    double setPosition_cov_xy;
    double setPosition_cov_r;



};

#endif // ROSGUIAPI_H
